// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: listener code decoupling
// feature: change E2E packet format
// feature: refacting  dispatcher discover
// feature: Support Cache fast application and slow release mechanism
// feature: RemoteEndpointInfo support reliabilityKind
// feature: develop dispatcher
// feature: content filter topic
// feature: Split history as an independent common module
// feature: Unified Transport Abstraction Layer Unified Transport Abstraction Layer
// feature: add statictic lookup
// feature: Crop code for in-process communication
// feature: E2E New Requirement Development
// feature: Development of new requirements for E2E functionality
// ------------------------------------------------------------------

#include <edds/rtps/writer/StatelessWriter.h>
#include <history/RemoteEndpointInfo.h>
#include <history/WriterListener.h>
#include <history/WriterHistory.h>
#include <edds/rtps/builtin/BuiltinProtocols.h>
#include <edds/rtps/builtin/liveliness/WLP.h>
#include <rtps/participant/RTPSParticipantImpl.h>
#include <history/CacheChangePool.h>
#include <elog/log/Log.h>

#include <algorithm>
#include <mutex>
#include <set>
#include <vector>

#include <elog/Log.hpp>
#include <history/BasicPayloadPool.hpp>
#include <history/HistoryAttributesExtension.hpp>
#include <rtps/network/ExternalLocatorsProcessor.hpp>
#include <rtps/RTPSDomainImpl.hpp>

#include "../flowcontrol/FlowController.hpp"
#include <deps/resources/TimedEvent.h>
#include <ertps/utils/LocatorAlias.h>

#if !defined(_WIN32)

#include <StaticTracepoint.h>
#include <tracepoint/TracePointManager.hpp>
#include <deps/common/SystemInfo.hpp>

#define SEC(name) __attribute__((section(name), used))
unsigned short ertps_ertps_less_writer_udp_send_semaphore SEC(".probes");

using evbs::ertps::rtps::c_Version_vbs;
using evbs::ertps::rtps::ReaderLocator;
void usdt_in_stateless_writer(CacheChange_t* change, const std::string& topic_name, uint8_t trace_point_status,
                              uint32_t domain_id,
                              const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& matched_remote_readers) {
    auto persistent_trace_point_status =
        vbsutil::tracepoint::TracePointManager::get_persistent_trace_point_status(topic_name);
    if (persistent_trace_point_status != vbsutil::tracepoint::INVALID &&
        persistent_trace_point_status > trace_point_status) {
        trace_point_status = persistent_trace_point_status;
    }
    change->trace_point_status = trace_point_status;
    switch (trace_point_status) {
        case vbsutil::tracepoint::OFF:
            logDebug(RTPS_WRITER, "Trace point status is OFF");
            break;
        case vbsutil::tracepoint::ON_ENTRANCE: {
            if (ertps_ertps_less_writer_udp_send_semaphore) {
                logDebug(RTPS_WRITER, "Trace point status is ON_ENTRANCE");
                uint64_t seq_num = change->sequenceNumber.to64long();
                uint64_t timestamp = change->sourceTimestamp.to_ns();
                octet* serialized_data = change->serializedPayload.data;
                uint32_t data_size = change->serializedPayload.length;
                uint32_t vbs_version =
                    (static_cast<uint32_t>(c_Version_vbs[0]) | (static_cast<uint32_t>(c_Version_vbs[1]) << 8) |
                     (static_cast<uint32_t>(c_Version_vbs[2]) << 16) | (static_cast<uint32_t>(c_Version_vbs[3]) << 24));
                uint32_t src_ip = 0;
                uint32_t dst_ip = 0;
                uint32_t mask_len = 0;
                if (matched_remote_readers.size() > 0) {
                    auto tmp_addr = matched_remote_readers[0]->general_locator_selector_entry()->unicast[0].address;
                    uint32_t remote_ip =
                        static_cast<uint32_t>(tmp_addr[12]) | (static_cast<uint32_t>(tmp_addr[13]) << 8) |
                        (static_cast<uint32_t>(tmp_addr[14]) << 16) | (static_cast<uint32_t>(tmp_addr[15]) << 24);
                    if (vbsutil::tracepoint::TracePointManager::find_same_subnet_ip(remote_ip, src_ip, mask_len)) {
                        if (matched_remote_readers.size() == 1) {
                            dst_ip = remote_ip;
                        } else {
                            dst_ip = 0;
                        }
                        vbsutil::tracepoint::UsdtInfo usdt_info = {
                            seq_num, timestamp, vbs_version, domain_id,
                            src_ip,  dst_ip,    mask_len,    vbsutil::tracepoint::ROLE_PUB};
                        unsigned char* usdt_msg_addr = reinterpret_cast<unsigned char*>(&usdt_info);
                        FOLLY_SDT_WITH_SEMAPHORE4(ertps, ertps_less_writer_udp_send, usdt_msg_addr, topic_name.c_str(),
                                                  serialized_data, data_size);
                    }
                }
            }
        } break;
        case vbsutil::tracepoint::ON_EXPORT: {
            logDebug(RTPS_WRITER, "Trace point status is ON_EXPORT");
            change->topic_name = topic_name;
            uint32_t vbs_version =
                (static_cast<uint32_t>(c_Version_vbs[0]) | (static_cast<uint32_t>(c_Version_vbs[1]) << 8) |
                 (static_cast<uint32_t>(c_Version_vbs[2]) << 16) | (static_cast<uint32_t>(c_Version_vbs[3]) << 24));
            change->vbs_version = vbs_version;
        } break;
        case vbsutil::tracepoint::DISABLE:
            logDebug(RTPS_WRITER, "Trace point status is DISABLE");
            break;
        case vbsutil::tracepoint::INVALID:
            logDebug(RTPS_WRITER, "Trace point status is INVALID");
            break;
    }
}

#endif

using evbs::ertps::rtps::DeliveryRetCode;
using evbs::ertps::rtps::RTPSMessageGroup;
using evbs::ertps::rtps::StatelessWriter;
using namespace vbs::common;
namespace evbs {
namespace ertps {
namespace rtps {
using edds::rtps::ExternalLocatorsProcessor::filter_remote_locators;
/**
 * Loops over all the readers in the vector, applying the given routine.
 * The loop continues until the result of the routine is true for any reader
 * or all readers have been processes.
 * The returned value is true if the routine returned true at any point,
 * or false otherwise.
 */
bool for_matched_readers(ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_1,
                         std::function<bool(ReaderLocator&)> fun) {
    for (auto& remote_reader : reader_vector_1) {
        if (fun(*remote_reader)) {
            return true;
        }
    }

    return false;
}

bool for_matched_readers(ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_1,
                         ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_2,
                         std::function<bool(ReaderLocator&)> fun) {
    if (for_matched_readers(reader_vector_1, fun)) {
        return true;
    }
    return for_matched_readers(reader_vector_2, std::move(fun));
}

bool for_matched_readers(ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_1,
                         ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_2,
                         ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_3,
                         std::function<bool(ReaderLocator&)> fun) {
    if (for_matched_readers(reader_vector_1, reader_vector_2, fun)) {
        return true;
    }
    return for_matched_readers(reader_vector_3, std::move(fun));
}

/**
 * Loops over all the readers in the vector, applying the given routine.
 * The loop continues until the result of the routine is true for any reader
 * or all readers have been processes.
 * The returned value is true if the routine returned true at any point,
 * or false otherwise.
 *
 * const version
 */
bool for_matched_readers(const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_1,
                         std::function<bool(const ReaderLocator)> fun) {
    for (const auto& remote_reader : reader_vector_1) {
        if (fun(*remote_reader)) {
            return true;
        }
    }

    return false;
}

bool for_matched_readers(const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_1,
                         const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_2,
                         std::function<bool(const ReaderLocator)> fun) {
    if (for_matched_readers(reader_vector_1, fun)) {
        return true;
    }
    return for_matched_readers(reader_vector_2, std::move(fun));
}

bool for_matched_readers(const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_1,
                         const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_2,
                         const ResourceLimitedVector<std::unique_ptr<ReaderLocator>>& reader_vector_3,
                         std::function<bool(const ReaderLocator&)> fun) {
    if (for_matched_readers(reader_vector_1, reader_vector_2, fun)) {
        return true;
    }
    return for_matched_readers(reader_vector_3, std::move(fun));
}

StatelessWriter::StatelessWriter(RTPSParticipantImpl* impl, const GUID_t& guid, const WriterAttributes& attributes,
                                 edds::rtps::FlowController* flow_controller, WriterHistory* history,
                                 WriterListener* listener)
    : RTPSWriter(impl, guid, attributes, flow_controller, history, listener),
      matched_remote_readers_(attributes.matched_readers_allocation),
#ifdef INTRA_PROCESS_ENABLE
      matched_local_readers_(attributes.matched_readers_allocation),
#endif
      matched_readers_pool_(attributes.matched_readers_allocation),
      locator_selector_(*this, attributes.matched_readers_allocation) {
    init(impl, attributes);
}

StatelessWriter::StatelessWriter(RTPSParticipantImpl* impl, const GUID_t& guid, const WriterAttributes& attributes,
                                 const std::shared_ptr<IPayloadPool>& payload_pool,
                                 edds::rtps::FlowController* flow_controller, WriterHistory* history,
                                 WriterListener* listener)
    : RTPSWriter(impl, guid, attributes, payload_pool, flow_controller, history, listener),
      matched_remote_readers_(attributes.matched_readers_allocation),
#ifdef INTRA_PROCESS_ENABLE
      matched_local_readers_(attributes.matched_readers_allocation),
#endif
      matched_readers_pool_(attributes.matched_readers_allocation),
      locator_selector_(*this, attributes.matched_readers_allocation) {
    init(impl, attributes);
}

StatelessWriter::StatelessWriter(RTPSParticipantImpl* participant, const GUID_t& guid,
                                 const WriterAttributes& attributes, const std::shared_ptr<IPayloadPool>& payload_pool,
                                 const std::shared_ptr<IChangePool>& change_pool,
                                 edds::rtps::FlowController* flow_controller, WriterHistory* history,
                                 WriterListener* listener)
    : RTPSWriter(participant, guid, attributes, payload_pool, change_pool, flow_controller, history, listener),
      matched_remote_readers_(attributes.matched_readers_allocation),
#ifdef INTRA_PROCESS_ENABLE
      matched_local_readers_(attributes.matched_readers_allocation),
#endif
      matched_readers_pool_(attributes.matched_readers_allocation),
      locator_selector_(*this, attributes.matched_readers_allocation) {
    init(participant, attributes);
}

void StatelessWriter::init(RTPSParticipantImpl* participant, const WriterAttributes& attributes) {
    get_builtin_guid();

    const RemoteLocatorsAllocationAttributes& loc_alloc =
        participant->getRTPSParticipantAttributes().allocation.locators;

    for (size_t i = 0; i < attributes.matched_readers_allocation.initial; ++i) {
        (void)matched_readers_pool_.emplace_back(
            new ReaderLocator(this, loc_alloc.max_unicast_locators, loc_alloc.max_multicast_locators));
    }

    if ((!this->m_guid.is_builtin()) && mp_history->autoMemoryMode) {
        mem_watermark = mp_history->m_att.maximumReservedCaches > 0 ? mp_history->m_att.maximumReservedCaches : 5000;
        free_event_ = new TimedEvent(
            participant->getEventResource(), [&]() -> bool { return history_adjustment(); }, 30000);
        free_event_->restart_timer();
    } else {
        free_event_ = nullptr;
    }
}

StatelessWriter::~StatelessWriter() {
    logDebug(RTPS_WRITER, "StatelessWriter destructor";);
    deinit();
    if (free_event_ != nullptr) {
        delete (free_event_);
        free_event_ = nullptr;
    }
}

void StatelessWriter::get_builtin_guid() {
    if (m_guid.entityId == ENTITYID_SPDP_BUILTIN_RTPSParticipant_WRITER) {
        add_guid(locator_selector_, GUID_t {GuidPrefix_t(), c_EntityId_SPDPReader});
#if HAVE_SECURITY
    } else if (m_guid.entityId == ENTITYID_P2P_BUILTIN_PARTICIPANT_STATELESS_WRITER) {
        add_guid(locator_selector_, GUID_t {GuidPrefix_t(), participant_stateless_message_reader_entity_id});
#endif  // if HAVE_SECURITY
    }
}

bool StatelessWriter::has_builtin_guid() {
    if (m_guid.entityId == ENTITYID_SPDP_BUILTIN_RTPSParticipant_WRITER) {
        return true;
    }
#if HAVE_SECURITY
    if (m_guid.entityId == ENTITYID_P2P_BUILTIN_PARTICIPANT_STATELESS_WRITER) {
        return true;
    }
#endif  // if HAVE_SECURITY
    return false;
}

void StatelessWriter::update_reader_info(bool create_sender_resources) {
    bool addGuid = !has_builtin_guid();
#ifdef INTRA_PROCESS_ENABLE
    (void)for_matched_readers(matched_local_readers_, matched_remote_readers_, [this](const ReaderLocator& reader) {
#else
    (void)for_matched_readers(matched_remote_readers_, [this](const ReaderLocator& reader) {
#endif
        UNUSED_PARAMETER(reader);
        return false;
    });

    update_cached_info_nts(locator_selector_);
    if (addGuid) {
        compute_selected_guids(locator_selector_);
    }

    if (create_sender_resources) {
        RTPSParticipantImpl* part = mp_RTPSParticipant;
        locator_selector_.locator_selector.for_each([part](const Locator_t& loc) { part->createSenderResources(loc); });
    }
}

void StatelessWriter::unsent_change_added_to_history(
    CacheChange_t* change, const std::chrono::time_point<std::chrono::steady_clock>& max_blocking_time,
    bool in_history) {
    auto payload_length = change->serializedPayload.length;
    std::unique_lock<RecursiveTimedMutex> guard(mp_mutex);
    if (liveliness_lease_duration_ < c_TimeInfinite) {
        (void)mp_RTPSParticipant->wlp()->assert_liveliness(getGuid(), liveliness_kind_, liveliness_lease_duration_);
    }

    // Now for the rest of readers
    if (!fixed_locators_.empty() || (getMatchedReadersSize() > 0)) {
#if !defined(_WIN32)
        usdt_in_stateless_writer(change, getTopicName(), getTracePointStatus(), getRTPSParticipant()->get_domain_id(),
                                 matched_remote_readers_);
#endif
        if (isAsync()) {
            (void)flow_controller_->add_new_sample(this, change, max_blocking_time);
        } else {
            // This call should be made with writer's mutex locked.
            std::lock_guard<ertps::rtps::LocatorSelectorSender> lock(locator_selector_);
            MessageLog(getTopicName(), change->writerGUID, MSG_TRACE_TYPE_SEND_OUT, change->sequenceNumber.to64long(),
                       change->sourceTimestamp, change->serializedPayload.length);
            ertps::rtps::RTPSMessageGroup group(mp_RTPSParticipant, this, &locator_selector_);
            (void)deliver_sample_nts(change, group, locator_selector_, max_blocking_time, in_history);
        }
    } else {
        logDebug(RTPS_WRITER, getTopicName() << " no reader to add change " << change->sequenceNumber << " .");
        if (in_history && mp_listener != nullptr) {
            mp_listener->onWriterChangeReceivedByAll(this, change);
        }
    }
    guard.unlock();
#if defined(EDDS_METRICS)
    on_send_frequency(ReliabilityKind_t::BEST_EFFORT, getGuid());
    on_send_throughput(ReliabilityKind_t::BEST_EFFORT, getGuid(), payload_length);
#else
    (void)payload_length;
#endif
}

bool StatelessWriter::intraprocess_delivery(CacheChange_t* change, ReaderLocator& reader_locator) {
    RTPSReader* reader = reader_locator.local_reader();

    if (reader) {
        if (change->write_params.related_sample_identity() != SampleIdentity::unknown()) {
            (void)change->write_params.sample_identity(change->write_params.related_sample_identity());
        }
        return reader->processDataMsg(change);
    }

    return false;
}

bool StatelessWriter::change_removed_by_history(CacheChange_t* change) {
    std::lock_guard<RecursiveTimedMutex> guard(mp_mutex);

    flow_controller_->remove_change(change);

    const uint64_t sequence_number = change->sequenceNumber.to64long();
    if (sequence_number > last_sequence_number_sent_) {
        unsent_changes_cond_.notify_all();
    }

    return true;
}

bool StatelessWriter::has_been_fully_delivered(const SequenceNumber_t& seq_num) const {
    // Sequence number has not been generated by this WriterHistory
    {
        std::lock_guard<RecursiveTimedMutex> guard(mp_history->getMutex());
        if (seq_num >= mp_history->next_sequence_number_nts()) {
            return false;
        }
    }

    if (getMatchedReadersSize() > 0) {
        return is_acked_by_all(seq_num);
    }
    return true;
}

bool StatelessWriter::is_acked_by_all(const CacheChange_t* change) const {
    return change->sequenceNumber.to64long() >= last_sequence_number_sent_;
}

bool StatelessWriter::is_acked_by_all(const SequenceNumber_t& seq_num) const {
    return seq_num.to64long() <= last_sequence_number_sent_;
}

bool StatelessWriter::try_remove_change(const std::chrono::steady_clock::time_point&,
                                        std::unique_lock<RecursiveTimedMutex>&) {
    bool ret;
#ifdef BATCH_SEND_ENABLE
    if (mp_history->batch_qos_.enable) {
        ret = mp_history->remove_min_batch_change(mp_history->next_sequence_number() - 1U);
    } else {
#else
    {
#endif
        ret = mp_history->remove_min_change();
    }
    return ret;
}

bool StatelessWriter::wait_for_acknowledgement(const SequenceNumber_t& seq,
                                               const std::chrono::steady_clock::time_point& max_blocking_time_point,
                                               std::unique_lock<RecursiveTimedMutex>& /*lock*/) {
    uint64_t sequence_number = seq.to64long();
    std::unique_lock<RecursiveTimedMutex> writer_lock(mp_mutex);
    auto change_is_acknowledged = [this, sequence_number]() {
        return sequence_number >= last_sequence_number_sent_;
    };
    return unsent_changes_cond_.wait_until(writer_lock, max_blocking_time_point, change_is_acknowledged);
}

/*
 *	MATCHED_READER-RELATED METHODS
 */
bool StatelessWriter::matched_reader_add(ReaderProxyData& rdata) {

    std::unique_lock<RecursiveTimedMutex> guard(mp_mutex);
    std::unique_lock<LocatorSelectorSender> locator_selector_guard(locator_selector_);

    assert(rdata.guid() != c_Guid_Unknown);
#ifdef INTRA_PROCESS_ENABLE
    if (for_matched_readers(matched_local_readers_, matched_remote_readers_, [this, &rdata](ReaderLocator& reader) {
#else
    ReaderLocator* found_reader = nullptr;
    if (for_matched_readers(matched_remote_readers_, [this, &rdata, &found_reader](ReaderLocator& reader) {
#endif
            if (reader.remote_guid() == rdata.guid()) {
                logDebug(RTPS_WRITER, "Topic " << getTopicName()
                                               << " attempting to add existing reader, updating information."
                                               << " remote guid:" << rdata.guid());
                found_reader = &reader;
                return true;
            }
            return false;
        })) {
        if (nullptr != mp_listener) {
            // call the listener without locks taken
            locator_selector_guard.unlock();
            guard.unlock();

            vbs::RemoteEndpointInfo rinfo;
            rinfo.guid(rdata.guid());
            rinfo.topicName(rdata.topicName());
            rinfo.typeName(rdata.typeName());
            rinfo.content_filter_property(rdata.content_filter());
            rinfo.topicKind(rdata.topicKind());
            rinfo.reliabilityKind(ReliabilityKind_t::BEST_EFFORT);
            rinfo.type_plain(rdata.type_plain());
            rinfo.host_id(rdata.get_host_id());
            rinfo.process_id(rdata.get_pid());

            uint16_t local_host_id = m_guid.guidPrefix.is_guid_static() ? vbs::SystemInfo::instance().host_id()
                                                                        : m_guid.guidPrefix.get_host_id();
            for (auto loc : rdata.remote_locators().unicast) {
                //本机之间通信，只上报SHM类型locator
                if (loc.kind == LOCATOR_KIND_DSF &&
                    m_guid.is_on_same_host_as(local_host_id, rdata.guid(), rdata.get_host_id())) {
                    rinfo.set_locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_DSF);
                    rinfo.locator(loc);
                    break;
                } else if (loc.kind == LOCATOR_KIND_UDPv4) {
                    rinfo.set_locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP);
                } else if (loc.kind == LOCATOR_KIND_UDS) {
                    rinfo.set_locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS);
                }
            }

            mp_listener->on_reader_discovery(READER_DISCOVERY_STATUS::CHANGED_QOS_READER, rdata.guid(), rinfo);
            // get_matched_status()的保障已经正常通信的实体间不更新locator信息，例如：本地writer和本地reader通过SHM通信，
            // 远端reader上线，本地writer会创建udp transport，然后远端writer上线，本地reader会更新edp携带udp，这时
            // 本地writer和本地reader会通过udp进行通信
            // locator类型为DSF时，直接进行处理，是因为simple模式初始edp默认带udp locator，如果发现是域内通信，第二个edp
            // 只携带dsf locator，所以需要进行过滤更新
            if (!rinfo.get_matched_status() &&
                ((!rdata.is_filtered(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP) &&
                  rinfo.locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP)) ||
                 (!rdata.is_filtered(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS) &&
                  rinfo.locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS)) ||
                 rinfo.locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_DSF))) {
                FilterReaderLocators(rdata, found_reader);
            }

            if (!this->m_guid.is_builtin()) {
                elogInfo(RTPS_WRITER, "Remote reader locator when CHANGED_QOS_READER: " << rdata.remote_locators()
                                                                                        << " guid: " << rdata.guid());
            }
        }
        return false;
    }

    // Get a locator from the inactive pool (or create a new one if necessary and allowed)
    std::unique_ptr<ReaderLocator> new_reader;
    ReaderLocator* newReader;
    if (matched_readers_pool_.empty()) {
        size_t max_readers = matched_readers_pool_.max_size();
        if (getMatchedReadersSize() + matched_readers_pool_.size() < max_readers) {
            const RemoteLocatorsAllocationAttributes& loc_alloc =
                mp_RTPSParticipant->getRTPSParticipantAttributes().allocation.locators;

            new_reader.reset(new ReaderLocator(this, loc_alloc.max_unicast_locators, loc_alloc.max_multicast_locators));
        } else {
            logWarning(RTPS_WRITER,
                       "Topic " << getTopicName() << " couldn't add matched reader due to resource limits");
            return false;
        }
    } else {
        new_reader = std::move(matched_readers_pool_.back());
        matched_readers_pool_.pop_back();
    }
    newReader = new_reader.get();
    // Add info of new rdatareader.
    (void)new_reader->start(rdata.guid(), rdata.remote_locators().unicast, rdata.remote_locators().multicast);
    filter_remote_locators(*new_reader->general_locator_selector_entry(), m_att.external_unicast_locators,
                           m_att.ignore_non_matching_locators);

    (void)locator_selector_.locator_selector.add_entry(new_reader->general_locator_selector_entry());
#ifdef INTRA_PROCESS_ENABLE
    if (new_reader->is_local_reader()) {
        (void)matched_local_readers_.push_back(std::move(new_reader));
        logDebug(RTPS_WRITER,
                 "Adding reader " << rdata.guid() << " to " << this->m_guid.entityId << " as local reader");
    } else {
#else
    {
#endif
        (void)matched_remote_readers_.push_back(std::move(new_reader));
        logDebug(RTPS_WRITER,
                 "Adding reader " << rdata.guid() << " to " << this->m_guid.entityId << " as remote reader");
    }

    update_reader_info(true);

    if (nullptr != mp_listener) {
        // call the listener without locks taken
        locator_selector_guard.unlock();
        guard.unlock();

        vbs::RemoteEndpointInfo rinfo;
        rinfo.guid(rdata.guid());
        rinfo.topicName(rdata.topicName());
        rinfo.typeName(rdata.typeName());
        rinfo.content_filter_property(rdata.content_filter());
        rinfo.topicKind(rdata.topicKind());
        rinfo.reliabilityKind(ReliabilityKind_t::BEST_EFFORT);
        rinfo.type_plain(rdata.type_plain());
        rinfo.host_id(rdata.get_host_id());
        rinfo.process_id(rdata.get_pid());

        for (auto loc : rdata.remote_locators().unicast) {
            if (loc.kind == LOCATOR_KIND_DSF) {
                rinfo.set_locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_DSF);
                rinfo.locator(loc);
            } else if (loc.kind == LOCATOR_KIND_UDPv4) {
                rinfo.set_locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP);
            } else if (loc.kind == LOCATOR_KIND_UDS) {
                rinfo.set_locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS);
            }
        }

        mp_listener->on_reader_discovery(READER_DISCOVERY_STATUS::DISCOVERED_READER, rdata.guid(), rinfo);

        //补充转换和过滤udp 或 uds locator
        if ((!rdata.is_filtered(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP) &&
             rinfo.locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP)) ||
            (!rdata.is_filtered(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS) &&
             rinfo.locator_valid(vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS))) {
            FilterReaderLocators(rdata, newReader);
        }

        if (!this->m_guid.is_builtin()) {
            elogInfo(RTPS_WRITER, "Remote reader locator when DISCOVERED_READER: " << rdata.remote_locators()
                                                                                   << " guid: " << rdata.guid());
        }
    }
    return true;
}

bool StatelessWriter::set_fixed_locators(const LocatorList_t& locator_list) {
#if HAVE_SECURITY
    if (getAttributes().security_attributes().is_submessage_protected ||
        getAttributes().security_attributes().is_payload_protected) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_ILLEGAL_OPERATION,  // LCOV_EXCL_START
                  "A secure besteffort writer cannot add a lonely locator");
        return false;
    }   // LCOV_EXCL_STOP
#endif  // if HAVE_SECURITY

    std::lock_guard<RecursiveTimedMutex> guard(mp_mutex);

    fixed_locators_.push_back(locator_list);
    mp_RTPSParticipant->createSenderResources(fixed_locators_);

    return true;
}

bool StatelessWriter::matched_reader_remove(const GUID_t& reader_guid) {
    std::unique_lock<RecursiveTimedMutex> guard(mp_mutex);
    std::unique_lock<LocatorSelectorSender> locator_selector_guard(locator_selector_);

    if (locator_selector_.locator_selector.remove_entry(reader_guid)) {
        std::unique_ptr<ReaderLocator> reader;
#ifdef INTRA_PROCESS_ENABLE
        for (auto it = matched_local_readers_.begin(); it != matched_local_readers_.end(); ++it) {
            if ((*it)->remote_guid() == reader_guid) {
                reader = std::move(*it);
                (void)matched_local_readers_.erase(it);
                break;
            }
        }
#endif
        if (reader == nullptr) {
            for (auto it = matched_remote_readers_.begin(); it != matched_remote_readers_.end(); ++it) {
                if ((*it)->remote_guid() == reader_guid) {
                    reader = std::move(*it);
                    (void)matched_remote_readers_.erase(it);
                    break;
                }
            }
        }

        // guid should be both on locator_selector_ and matched_readers_ or in none
        assert(reader != nullptr);

        if (reader != nullptr) {
            vbs::RemoteEndpointInfo rinfo;
            rinfo.type_plain(reader->type_plain());
            rinfo.domainId(mp_RTPSParticipant->get_domain_id());
            rinfo.guid(reader_guid);

            reader->stop();
            (void)matched_readers_pool_.push_back(std::move(reader));
            update_reader_info(false);
            logDebug(RTPS_WRITER, "Reader Proxy removed: " << reader_guid);
            if (nullptr != mp_listener) {
                // call the listener without locks taken
                locator_selector_guard.unlock();
                guard.unlock();

                mp_listener->on_reader_discovery(READER_DISCOVERY_STATUS::REMOVED_READER, reader_guid, rinfo);
            }
            return true;
        }
    }

    return false;
}

bool StatelessWriter::matched_reader_is_matched(const GUID_t& reader_guid) {
    std::lock_guard<RecursiveTimedMutex> guard(mp_mutex);
    return for_matched_readers(
#ifdef INTRA_PROCESS_ENABLE
        matched_local_readers_, matched_remote_readers_,
#else
        matched_remote_readers_,
#endif
        [reader_guid](const ReaderLocator& reader) { return reader.remote_guid() == reader_guid; });
}

void StatelessWriter::unsent_changes_reset() {
    std::lock_guard<RecursiveTimedMutex> guard(mp_mutex);
    (void)std::for_each(mp_history->changesBegin(), mp_history->changesEnd(), [&](CacheChange_t* change) {
        (void)flow_controller_->add_new_sample(this, change, std::chrono::steady_clock::now() + std::chrono::hours(24));
    });
}

bool StatelessWriter::send_nts(CDRMessage_t* message, const LocatorSelectorSender& locator_selector) const {
    if (!RTPSWriter::send_nts(message, locator_selector)) {
        return false;
    }

    return fixed_locators_.empty() || mp_RTPSParticipant->sendSync(message, m_guid, Locators(fixed_locators_.begin()),
                                                                   Locators(fixed_locators_.end()), is_send_multi);
}

size_t StatelessWriter::getMatchedReadersSize() const {
    const std::lock_guard<RecursiveTimedMutex> guard(const_cast<RecursiveTimedMutex&>(mp_mutex));
#ifdef INTRA_PROCESS_ENABLE
    return matched_remote_readers_.size() + matched_local_readers_.size();
#else
    return matched_remote_readers_.size();
#endif
}

DeliveryRetCode StatelessWriter::deliver_sample_nts(
    CacheChange_t* cache_change, RTPSMessageGroup& group,
    LocatorSelectorSender& locator_selector,  // Object locked by FlowControllerImpl
    const std::chrono::time_point<std::chrono::steady_clock>& /*TODO max_blocking_time*/, bool in_history) {
    uint64_t change_sequence_number = cache_change->sequenceNumber.to64long();
    DeliveryRetCode ret_code = DeliveryRetCode::DELIVERED;

    cache_change->isStateless = true;

    if (current_sequence_number_sent_ != change_sequence_number) {
        current_sequence_number_sent_ = change_sequence_number;
        current_fragment_sent_ = 0;
    }
#ifdef INTRA_PROCESS_ENABLE
    // Send the new sample to intra-process readers.
    if (0 == current_fragment_sent_) {
        for_matched_readers(matched_local_readers_, [&, cache_change](ReaderLocator& reader) {
#ifdef BATCH_SEND_ENABLE
            if (cache_change->batch_send) {
                for (auto it = cache_change->batch_changes.begin(); it != cache_change->batch_changes.end(); it++) {
                    (void)intraprocess_delivery(*it, reader);
                }
            } else {
#else
            {
#endif
                (void)intraprocess_delivery(cache_change, reader);
            }
            return false;
        });
    }
#endif
    octet align = static_cast<octet>((4U - cache_change->serializedPayload.length % 4U) & 3U);
    if (cache_change->serializedPayload.length > 4) {
        cache_change->serializedPayload.data[3] = align;
    }

    try {
        uint32_t n_fragments = cache_change->getFragmentCount();

        if (m_separateSendingEnabled) {  // NOLINT
            if (0 < n_fragments) {
                for (FragmentNumber_t frag = current_fragment_sent_ + 1;
                     DeliveryRetCode::DELIVERED == ret_code && frag <= n_fragments; ++frag) {
                    for (std::unique_ptr<ReaderLocator>& it : matched_remote_readers_) {
                        if (reader_data_filter() &&
                            !reader_data_filter()->is_relevant(*cache_change, it->remote_guid())) {
                            continue;
                        }
                        group.sender(this, &*it);

                        if (group.add_data_frag(*cache_change, frag, enable_e2e_protection_, e2e_profile04_)) {
                        } else {
                            elogErrorKeyT(RTPS_WRITER, RetCode_t::RETCODE_NOT_DELIVERED, getTopicName(),
                                          "Topic " << getTopicName() << " sending fragment ("
                                                   << cache_change->sequenceNumber << ", " << frag << ") fail.");
                            ret_code = DeliveryRetCode::NOT_DELIVERED;
                        }
                    }

                    if (DeliveryRetCode::DELIVERED == ret_code) {
                        current_fragment_sent_ = frag;
                    }
                }
            } else {
                bool bSingleReader = false;
                for (std::unique_ptr<ReaderLocator>& it : matched_remote_readers_) {
                    if (reader_data_filter() && !reader_data_filter()->is_relevant(*cache_change, it->remote_guid())) {
                        continue;
                    }
                    if (cache_change->readerGUID != c_Guid_Unknown) {
                        bSingleReader = true;
                        if (cache_change->readerGUID.guidPrefix != it->remote_guid().guidPrefix) {
                            continue;
                        }
                    }
                    group.sender(this, &*it);

                    if (!group.add_data(*cache_change, enable_e2e_protection_, e2e_profile04_)) {
                        elogErrorKeyT(RTPS_WRITER, RetCode_t::RETCODE_NOT_DELIVERED, getTopicName(),  // LCOV_EXCL_START
                                      "Topic " << getTopicName() << " sending change " << cache_change->sequenceNumber
                                               << " fail.");
                        ret_code = DeliveryRetCode::NOT_DELIVERED;
                    }  // LCOV_EXCL_STOP
                    if (bSingleReader) {
                        break;
                    }
                }
            }
        } else {
            if (reader_data_filter()) {
                locator_selector.locator_selector.reset(false);
                for (std::unique_ptr<ReaderLocator>& it : matched_remote_readers_) {
                    if (reader_data_filter()->is_relevant(*cache_change, it->remote_guid())) {
                        locator_selector.locator_selector.enable(it->remote_guid());
                    }
                }
            } else {
                locator_selector.locator_selector.reset(true);
            }

            if (locator_selector.locator_selector.state_has_changed()) {
                mp_RTPSParticipant->select_locators(locator_selector.locator_selector);
                if (!has_builtin_guid()) {
                    compute_selected_guids(locator_selector);
                }
            }

            size_t num_locators = locator_selector.locator_selector.selected_size() + fixed_locators_.size();

            if (0 < num_locators) {
                if (0 < n_fragments) {
                    for (FragmentNumber_t frag = current_fragment_sent_ + 1;
                         DeliveryRetCode::DELIVERED == ret_code && frag <= n_fragments; ++frag) {
                        if (group.add_data_frag(*cache_change, frag, enable_e2e_protection_, e2e_profile04_)) {
                            current_fragment_sent_ = frag;
                        } else {
                            elogErrorKeyT(RTPS_WRITER, RetCode_t::RETCODE_NOT_DELIVERED,
                                          getTopicName(),  // LCOV_EXCL_START
                                          "Topic " << getTopicName() << " sending fragment ("
                                                   << cache_change->sequenceNumber << ", " << frag << ") fail.");
                            ret_code = DeliveryRetCode::NOT_DELIVERED;
                        }  // LCOV_EXCL_STOP
                    }
                } else {
                    if (!group.add_data(*cache_change, enable_e2e_protection_, e2e_profile04_)) {
                        elogErrorKeyT(RTPS_WRITER, RetCode_t::RETCODE_NOT_DELIVERED, getTopicName(),  // LCOV_EXCL_START
                                      "Topic " << getTopicName() << " sending change " << cache_change->sequenceNumber
                                               << " fail.");
                        ret_code = DeliveryRetCode::NOT_DELIVERED;
                    }  // LCOV_EXCL_STOP
                }
            } else {
                logDebug(RTPS_WRITER, "Topic " << getTopicName() << " writer " << getGuid()
                                               << " sending change failed-No selected remote locator.");
            }
        }
    } catch (const RTPSMessageGroup::timeout&) {
        elogErrorKeyT(RTPS_WRITER, RetCode_t::RETCODE_TIMEOUT, getTopicName(),  // LCOV_EXCL_START
                      "Topic " << getTopicName() << " max blocking time reached");
        ret_code = DeliveryRetCode::NOT_DELIVERED;  // LCOV_EXCL_STOP
    } catch (const RTPSMessageGroup::limit_exceeded&) {
        ret_code = DeliveryRetCode::EXCEEDED_LIMIT;
    }

    group.sender(this, &locator_selector);

    if ((DeliveryRetCode::DELIVERED == ret_code) && (change_sequence_number > last_sequence_number_sent_)) {
        // This update must be done before calling the callback.
        last_sequence_number_sent_ = change_sequence_number;
        unsent_changes_cond_.notify_all();

        if (in_history && nullptr != mp_listener) {
            mp_listener->onWriterChangeReceivedByAll(this, cache_change);
        }
    }
    return ret_code;
}

evbs::edds::dds::builtin::StatisticMatchGuids StatelessWriter::get_remote_guids() {
    evbs::edds::dds::builtin::StatisticMatchGuids guids;

    std::unique_lock<RecursiveTimedMutex> guard(mp_mutex);
    for (uint32_t i = 0; i < matched_remote_readers_.size(); i++) {
        guids.push_back(matched_remote_readers_[i]->remote_guid());
    }
    return guids;
}

edds::dds::builtin::StatisticProxyInfos StatelessWriter::get_proxy_infos() {
    edds::dds::builtin::StatisticProxyInfos infos;

    std::unique_lock<RecursiveTimedMutex> guard(mp_mutex);
    for (uint32_t i = 0; i < matched_remote_readers_.size(); i++) {
        edds::dds::builtin::ProxyInfo info {};
        info.type = evbs::edds::dds::builtin::STATICTIC_ENTITY_READER;
        info.is_alive = true;
        info.start_time = matched_remote_readers_[i]->get_start_time();
        info.last_acknack_count = 0;
        info.last_nackfrag_count = 0;
        info.changes_low_mark = 0;
        info.last_heartbeat_count = 0;
        info.reader_proxy_async_locator_entry = *matched_remote_readers_[i]->async_locator_selector_entry();
        info.reader_proxy_general_locator_entry = *matched_remote_readers_[i]->general_locator_selector_entry();
        infos.push_back(std::move(info));
    }
    return infos;
}

bool StatelessWriter::history_adjustment() {
    mem_watermark += mp_history->getHistorySize();
    mem_watermark = mem_watermark / 2;
    mem_watermark = mem_watermark > 1 ? mem_watermark : 1;
    return payload_pool_->free_payload(mem_watermark);
}

void StatelessWriter::FilterReaderLocators(ReaderProxyData& rdata, ReaderLocator* reader) {
    LocatorList tmp_unicast;
    for (auto loc : rdata.remote_locators().unicast) {
        Locator_t temp_locator;
        vbs::RemoteEndpointInfo::LocatorType locator_type;
        if (loc.kind == LOCATOR_KIND_UDS) {
            locator_type = vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDS;
        } else if (loc.kind == LOCATOR_KIND_UDPv4) {
            locator_type = vbs::RemoteEndpointInfo::LocatorType::LOCATOR_TYPE_UDP;
        } else if (loc.kind == LOCATOR_KIND_DSF) {
            tmp_unicast.push_back(loc);
            continue;
        }
        if ((this->m_guid.is_builtin() || mp_RTPSParticipant->registed_user_transport(locator_type)) &&
            mp_RTPSParticipant->network_factory()->transform_remote_locator(mp_RTPSParticipant->get_transport_name(),
                                                                            loc, temp_locator) &&
            mp_RTPSParticipant->is_valid_remote_locator(loc)) {
            tmp_unicast.push_back(temp_locator);
        }
    }
    rdata.set_announced_unicast_locators(tmp_unicast);

    (void)reader->update(rdata.remote_locators().unicast, rdata.remote_locators().multicast);
    filter_remote_locators(*reader->general_locator_selector_entry(), m_att.external_unicast_locators,
                           m_att.ignore_non_matching_locators);

    update_reader_info(true);
}

} /* namespace rtps */
} /* namespace ertps */
} /* namespace evbs */
